#include "pstwo.h"
#define DELAY_TIME  delay_us(5); 



unsigned short Handkey;	


unsigned char Comd[2]={0x01,0x42};	


unsigned char PS_Data[9]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; 
int Beep_Control_Flag = 0;
unsigned short MASK[]={
    PSB_SELECT,
    PSB_L3,
    PSB_R3 ,
    PSB_START,
    PSB_PAD_UP,
    PSB_PAD_RIGHT,
    PSB_PAD_DOWN,
    PSB_PAD_LEFT,
    PSB_L2,
    PSB_R2,
    PSB_L1,
    PSB_R1 ,
    PSB_GREEN,
    PSB_RED,
    PSB_BLUE,
    PSB_PINK
	}; 

void pstwo_task(void *pvParameters)
{
	printf("_______>>>>>>>>> %s %d \r\n", __FILE__, __LINE__);
    while(1)
    {	
			
			
			LOS_Msleep(10);
			
			
      		PS2_Read(); 
			
			Servo_init_angle_adjust_mode_check();
    }
}  

void PS2_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStructure;
	
	
	
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;			
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;		
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;	
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;		
	GPIO_Init(GPIOE, &GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_10|GPIO_Pin_12;		  
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_OUT;          
	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;        
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;     
	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;         
	GPIO_Init(GPIOE, &GPIO_InitStructure);
}

void PS2_Read(void)
{
	static int Strat;

	
	PS2_KEY=PS2_DataKey(); 

	PS2_LX=PS2_AnologData(PSS_LX); 
	
  
	PS2_LY=PS2_AnologData(PSS_LY);
	
  
	PS2_RX=PS2_AnologData(PSS_RX);
	
  
	PS2_RY=PS2_AnologData(PSS_RY);  

	if(PS2_KEY==4&&PS2_ON_Flag==0)
	{
		Strat=1; 
		printf("____>>>>> %s %d \r\n", __FILE__, __LINE__);
		printf("PS2_LY %d PS2_ON_Flag %d Deviation_Count %d CONTROL_DELAY %d\r\n", PS2_LY, PS2_ON_Flag, Deviation_Count, CONTROL_DELAY);
	}
		
	
	//if(Strat&&(PS2_LY<118)&&PS2_ON_Flag==0&&Deviation_Count>=CONTROL_DELAY)
	if(Strat&&PS2_ON_Flag==0&&Deviation_Count>=CONTROL_DELAY)
	{
		printf("____>>>>> %s %d \r\n", __FILE__, __LINE__);
		PS2_ON_Flag=1,Remote_ON_Flag=0,APP_ON_Flag=0,CAN_ON_Flag=0,Usart1_ON_Flag=0,Usart5_ON_Flag=0;
	}
		  

}

void PS2_Cmd(unsigned char CMD)
{
	volatile unsigned short ref=0x01;
	PS_Data[1] = 0;
	for(ref=0x01;ref<0x0100;ref<<=1)
	{
		if(ref&CMD)
		{
			DO_H;     
		}
		else DO_L;

		CLK_H;      
		DELAY_TIME;
		CLK_L;
		DELAY_TIME;
		CLK_H;
		if(DI)
			PS_Data[1] = ref|PS_Data[1];
	}
	delay_us(16);
}

unsigned char PS2_RedLight(void)
{
	CS_L;
	PS2_Cmd(Comd[0]);  
	PS2_Cmd(Comd[1]);  
	CS_H;
	if( PS_Data[1] == 0X73)   return 0 ;
	else return 1;

}

void PS2_ReadData(void)
{
	volatile unsigned char byte=0;
	volatile unsigned short ref=0x01;
	CS_L;
	PS2_Cmd(Comd[0]);  
	PS2_Cmd(Comd[1]);  
	for(byte=2;byte<9;byte++) 
	{
		for(ref=0x01;ref<0x100;ref<<=1)
		{
			CLK_H;
			DELAY_TIME;
			CLK_L;
			DELAY_TIME;
			CLK_H;
		      if(DI)
		      PS_Data[byte] = ref|PS_Data[byte];
		}
        delay_us(16);
	}
	CS_H;
}

void go_null(void)
{
	int i, j;
	for(i = 0; i < 100; i++)
	{
		for(j = 0; j < 10; j++)
		{
			
		}
	}
}

unsigned char PS2_DataKey()
{
	unsigned char index;
	unsigned short temp;


	PS2_ClearData();
	PS2_ReadData();

	printf("");
	Handkey=(PS_Data[4]<<8);
	printf("");
	Handkey = Handkey |PS_Data[3];
	printf("");
	for(index=0;index<16;index++)
	{
		if((Handkey&(1<<(MASK[index]-1)))==0)
		{
			return index+1;
		}
	}
	return 0;  
}

unsigned char PS2_AnologData(unsigned char button)
{
	return PS_Data[button];
}

void PS2_ClearData()
{
	unsigned char a;
	for(a=0;a<9;a++)
		PS_Data[a]=0x00;
}

void PS2_Vibration(unsigned char motor1, unsigned char motor2)
{
	CS_L;
	delay_us(16);
  PS2_Cmd(0x01); 
	PS2_Cmd(0x42); 
	PS2_Cmd(0X00);
	PS2_Cmd(motor1);
	PS2_Cmd(motor2);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	CS_H;
	delay_us(16);  
}

void PS2_ShortPoll(void)
{
	CS_L;
	delay_us(16);
	PS2_Cmd(0x01);  
	PS2_Cmd(0x42);  
	PS2_Cmd(0X00);
	PS2_Cmd(0x00);
	PS2_Cmd(0x00);
	CS_H;
	delay_us(16);	
}

void PS2_EnterConfing(void)
{
  CS_L;
	delay_us(16);
	PS2_Cmd(0x01);  
	PS2_Cmd(0x43);  
	PS2_Cmd(0X00);
	PS2_Cmd(0x01);
	PS2_Cmd(0x00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	CS_H;
	delay_us(16);
}

void PS2_TurnOnAnalogMode(void)
{
	CS_L;
	PS2_Cmd(0x01);  
	PS2_Cmd(0x44);  
	PS2_Cmd(0X00);
	PS2_Cmd(0x01); 
	PS2_Cmd(0x03); 
				         
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	CS_H;
	delay_us(16);
}

void PS2_VibrationMode(void)
{
	CS_L;
	delay_us(16);
	PS2_Cmd(0x01);  
	PS2_Cmd(0x4D);  
	PS2_Cmd(0X00);
	PS2_Cmd(0x00);
	PS2_Cmd(0X01);
	CS_H;
	delay_us(16);	
}

void PS2_ExitConfing(void)
{
    CS_L;
	delay_us(16);
	PS2_Cmd(0x01);  
	PS2_Cmd(0x43);  
	PS2_Cmd(0X00);
	PS2_Cmd(0x00);
	PS2_Cmd(0x5A);
	PS2_Cmd(0x5A);
	PS2_Cmd(0x5A);
	PS2_Cmd(0x5A);
	PS2_Cmd(0x5A);
	CS_H;
	delay_us(16);
}

void PS2_SetInit(void)
{
	PS2_ShortPoll();
	PS2_ShortPoll();
	PS2_ShortPoll();
	PS2_EnterConfing();		  
	PS2_TurnOnAnalogMode();	
	
	PS2_ExitConfing();		  
}

void PS2_Receive (void)
{
	if(PS2_ON_Flag)
		{
		PS2_LX=PS2_AnologData(PSS_LX);
		PS2_LY=PS2_AnologData(PSS_LY);
		PS2_RX=PS2_AnologData(PSS_RX);
		PS2_RY=PS2_AnologData(PSS_RY);
		}
		PS2_KEY=PS2_DataKey();
}


void Servo_init_angle_adjust_mode_check(void)
{
	static int check_count1=0,check_count2=0;
	
		if(PS2_LX<100 && PS2_RX>200)   check_count1++,Move_X=0,Move_Y=0,Move_Z=0;  
	  else check_count1=0;
	
		if(PS2_LX>200 && PS2_RX<100)   check_count2++,Move_X=0,Move_Y=0,Move_Z=0;  
	  else check_count2=0;
	
	if(check_count1>500)
	{
		
		Beep_Control_Flag = 1;
		Servo_init_angle_adjust=1; 
		check_count1=0;
		
	}
		
	if(check_count2>500 && Servo_init_angle_adjust==1)
	{
		Beep_Control_Flag = 1;
		Servo_init_angle_adjust=0;
	  Flash_Write();     
		check_count2=0;
	}

}

void Servo_init_angle_adjust_function(void)
{
	int step=1; 
	static int once=0;
	
	if (once == 0)
	{
	  Moveit_Angle1=0,Moveit_Angle2=0,Moveit_Angle3=0,Moveit_Angle4=0;
		once=1;
	}
				if(PS2_KEY==5)     Moveit_Angle1_init+=step;     
	 else if(PS2_KEY==7)		 Moveit_Angle1_init-=step;
		
	 else if(PS2_KEY==6)		Moveit_Angle2_init-=step;     
	 else if(PS2_KEY==8)		Moveit_Angle2_init+=step;   
		
	 else if(PS2_KEY==13)		Moveit_Angle3_init+=step;   
	 else if(PS2_KEY==15)		Moveit_Angle3_init-=step;	
	
		 else if(PS2_KEY==14)		 Moveit_Angle4_init-=step;    
	 else if(PS2_KEY==16)		 Moveit_Angle4_init+=step;
}


